#pragma once
#ifndef _MAP_H_
#define _MAP_H_

#include "common_include.h"
#include "frame.h"
#include"utility.h"
#include"config.h"

namespace esikf_slam{

class Map {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<Map> Ptr;
    typedef unordered_map<unsigned long, Frame::Ptr> KeyFramesType;

    Frame::Ptr p_current_frame_ = nullptr;  //由于插入关键帧不一定都是新帧，因此，索引关键帧vector的话，不一定能找到 
    Config::Ptr p_config_;

    Map(Config::Ptr p_config){
        p_config_ = p_config;
    }

    void insertKeyFrame(Frame::Ptr p_frame);

    pcl::PointCloud<PointType>::Ptr getLocalMap();

    pcl::PointCloud<PointType>::Ptr getMap();

    void updateLocalmap();

private:
    KeyFramesType keyframes_, active_keyframes_;

    int num_active_keyframes_=3;

    pcl::PointCloud<PointType>::Ptr localmap_;

    // mutex data_mutex_;

    //移除旧的关键帧，滑动窗口，但是，还是需要边缘化，不然，会少一个约束
    void removeNotGoodKeyframe();

};


}
#endif



